function [PathPoints, st, ErrCode, info] = SPLNSNPath(vehx, vehy, vehth, tpmx, tpmy, tpmth, NR, PSInfo, VEHInfo,ParamInfo)
% 直行前进 + 左转后退 + 直行后退

% 输入数据处理
Cr2SafeDist = ParamInfo.Cr2SafeDist;
ymin = ParamInfo.ymin;
cr2x = PSInfo.cr2x;
cr2y = PSInfo.cr2y;
VehWidth = VEHInfo.VehWidth;
VehRear  = VEHInfo.VehRear;
VehLength = VEHInfo.VehLength;
VehWidth = VEHInfo.VehWidth;
Cr2SafeDist = ParamInfo.Cr2SafeDist;
Cr2obsx = PSInfo.cr2obsx;
Cr2obsy = PSInfo.cr2obsy;
% 初始化返回结果 
% 初始化返回结果

info = struct('Fcount',uint8(0),'Ncount',uint8(0),'count',uint8(0),'c2dist',single(0),'ymin',single(0));
info.count = uint8(0);
st = true;
ErrCode = uint8(0);
PathPoint  = struct('XPos',single(0),'YPos',single(0),'Headangle',single(0),'Kappa',single(0),'PathLength',single(0),'PathType',Pt_Typ_PathType_en.Initial);
PathPoints = repmat(PathPoint,[1 8]);

% 关键点计算
% 1 求交点Ix, Iy
a1 =  sin(deg2rad(vehth));
b1 = -cos(deg2rad(vehth));
c1 = vehy*cos(deg2rad(vehth)) - vehx*sin(deg2rad(vehth));

a2 =  sin(deg2rad(tpmth));
b2 = -cos(deg2rad(tpmth));
c2 = tpmy*cos(deg2rad(tpmth)) - tpmx*sin(deg2rad(tpmth));

[Ix, Iy, Ist] = P_intersectLines(a1, b1, c1, a2, b2, c2);
% 当veh和tpm没有交点时返回
if ~Ist
    st = false;
    ErrCode = uint8(1);
    return;
end
% 2 求切点1
dth = abs(OffCPIP_angdiff(deg2rad(vehth),deg2rad(tpmth)));
L = NR*tan(dth/single(2));
T1x  = Ix + L*cos(deg2rad(vehth));
T1y  = Iy + L*sin(deg2rad(vehth));
% 3 求切点2
T2x  = Ix - L*cos(deg2rad(tpmth));
T2y  = Iy - L*sin(deg2rad(tpmth));
% 判断切点是否在tpm点在后方
dotval = (T2x - tpmx)*cos(deg2rad(tpmth)) + (T2y - tpmy)*sin(deg2rad(tpmth));
% 第三段直线段不存在
if dotval < single(0)    
    st = false;
    ErrCode = uint8(2);
    return;
end
% 4 判断NP1的右前点是否小于ymin
NP1RFy = T1y + (VehLength - VehRear)*sin(deg2rad(vehth)) + VehWidth/single(2)*sin(deg2rad(vehth)-single(pi/2));
% NP1右前点超出ymin
if NP1RFy < ymin
    st = false;
    ErrCode = uint8(3);
    return;
end

% 5 求圆心2坐标
C2x  = T2x + NR*cos(deg2rad(tpmth)+single(pi/2));
C2y  = T2y + NR*sin(deg2rad(tpmth)+single(pi/2));
% 判断2点到圆弧段的最短距离是否满足碰撞要求
dis = NR - sqrt((C2x - Cr2obsx)^2 + (C2y - Cr2obsy)^2) - VehWidth/single(2);
% 圆弧段不满足碰撞要求
if dis < Cr2SafeDist
    st = false;
    ErrCode = uint8(4);
    return;
end
% 判断第一段直线是否过短
sl = sqrt((T1x-vehx)^2+(T1y-vehy)^2);
if sl < single(0.3)
    st = false;
    ErrCode = uint8(5);
    return;
end
% 返回结果更新
PathPoints(1).XPos = vehx;
PathPoints(1).YPos = vehy;
PathPoints(1).Headangle = vehth;
PathPoints(1).PathType = Pt_Typ_PathType_en.FLine;
PathPoints(1).Kappa    = single(0);
PathPoints(1).PathLength = sqrt((T1x-vehx)^2+(T1y-vehy)^2);

PathPoints(2).XPos = T1x;
PathPoints(2).YPos = T1y;
PathPoints(2).Headangle = vehth;
PathPoints(2).PathType = Pt_Typ_PathType_en.BLCircle;
PathPoints(2).Kappa    = single(1/NR);
PathPoints(2).PathLength = dth*NR;

PathPoints(3).XPos = T2x;
PathPoints(3).YPos = T2y;
PathPoints(3).Headangle = tpmth;
PathPoints(3).PathType = Pt_Typ_PathType_en.BLine;
PathPoints(3).Kappa    = single(0);
PathPoints(3).PathLength = sqrt((T2x-tpmx)^2+(T2y-tpmy)^2);

PathPoints(4).XPos = tpmx;
PathPoints(4).YPos = tpmy;
PathPoints(4).Headangle = tpmth;
PathPoints(4).PathType = Pt_Typ_PathType_en.Initial;
PathPoints(4).Kappa    = single(0);
PathPoints(4).PathLength = single(0);
info.count = uint8(3);
p1ok = OffCPIP_CheckKeyPathPoint(PathPoints(1),PathPoints(2));
p2ok = OffCPIP_CheckKeyPathPoint(PathPoints(2),PathPoints(3));
p3ok = OffCPIP_CheckKeyPathPoint(PathPoints(3),PathPoints(4));
if ~(p1ok&&p2ok&&p3ok)
    st = false;
end
end
    


